#include "../../include/arm/assembler.h"
#include "../../include/arm/v7m.h"
#include "../../include/asm/ptrace.h"

ENTRY(__pendsv_entry)
    cpsid	i

    mrs	r0, psp
    vstmdb r0!, {s16 - s31}
    stmfd r0!, {r4 - r11}     /* push r4 - r11 to prev-task sp */
    stmfd r0!, {lr}

    push {lr}
    bl __pendsv_do_switch /* return r0: next sp */
    pop {lr}

    ldmfd r0!, {lr}          /* restore lr from next-task sp */
    ldmfd r0!, {r4 - r11} /* restore r4 - r11 from next-task sp */
    vldmia r0!, {s16 - s31}
    msr psp, r0

    cpsie i
    bx lr
ENDPROC(__pendsv_entry)
